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ABSTRACT 

This  paper  focuses  on  multi-sensor  fusion  for  navigation  in  difficult  environments  where  none  of  the  existing 
navigation  technologies  can  satisfy  requirements  for  accurate  and  reliable  navigation  if  used  in  a  stand-alone 
mode.  A  generic  multi-sensor  fusion  approach  is  presented.  This  approach  builds  the  navigation 
mechanization  around  a  self-contained  inertial  navigator,  which  is  used  as  a  core  sensor.  Other  sensors 
generally  derive  navigation-related  measurements  from  external  signals,  such  as  Global  Navigation  Satellite 
System  (GNSS)  signals  and  signals  of  opportunity  (SOOP),  or  external  observations,  for  example,  features 
extracted  from  images  of  laser  scanners  and  video  cameras.  Depending  on  a  specific  navigation  mission, 
these  measurements  may  or  may  not  be  available.  Therefore,  externally-dependent  sources  of  navigation 
information  (including  GNSS,  SOOP,  laser  scanners,  video  cameras,  pseudolites,  Doppler  radars,  etc.)  are 
treated  as  secondary  sensors.  When  available,  measurements  of  a  secondary  sensor  or  sensors  are  utilized  to 
reduce  drift  in  inertial  navigation  outputs.  Inertial  data  are  applied  to  improve  the  robustness  of  secondary 
sensors  ’  signal  processing.  Applications  of  the  multi-sensor  fusion  approach  are  illustrated  in  details  for  two 
case  studies:  1)  integration  of  Global  Positioning  System  (GPS),  laser  scanner  and  inertial  navigation;  and, 
2)  fusion  of  laser  scanner,  video  camera,  and  inertial  measurements.  Experimental  and  simulation  results  are 
presented  to  illustrate  performance  of  multi-sensor  fusion  algorithms. 


1.0  MOTIVATION 

Many  existing  and  perspective  applications  of  navigation  systems  would  benefit  notably  from  the  ability  to 
navigate  accurately  and  reliably  in  difficult  environments.  Examples  of  difficult  navigation  scenarios  include 
urban  canyons,  indoor  applications,  radio-frequency  (RF)  interference  and  jamming  environments.  In 
addition,  different  segments  of  a  mission  path  can  impose  significantly  different  requirements  on  the 
navigation  sensing  technology  and  data  processing  algorithms.  To  exemplify,  Figure  1  shows  a  mission 
scenario  of  an  autonomous  aerial  vehicle  (UAV). 
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Figure  1.  UAV  mission  example 

For  this  example,  the  UAV  is  deployed  in  an  open  field;  next,  the  vehicle  enters  an  urban  canyon  to  perform 
tasks  such  as  surveillance  and  reconnaissance;  and,  finally,  it  returns  to  the  deployment  point.  To  enable 
operation  of  the  UAV  at  any  point  on  the  flight  path,  a  precision  navigation,  attitude,  and  time  capability  on¬ 
board  the  vehicle  is  required.  Global  Navigation  Satellite  System  (GNSS)  generally  provides  satisfactory 
performance  in  open  fields  and  suburban  areas,  but  has  fragmented  availability  in  urban  environments  due  to 
satellite  blockages  by  buildings  and  other  obstacles.  Feature-based  navigation  techniques  demonstrate  a 
promising  potential  in  dense  urban  areas  where  enough  navigation-related  features  can  be  extracted  from 
images  of  digital  cameras  or  laser  scanners.  Flowever,  the  feature  availability  can  be  limited  in  relatively  open 
areas.  A  self-contained  inertial  navigation  system  (INS)  can  provide  navigation  solution  at  any  environment; 
however,  the  solution  accuracy  drifts  over  time.  In  a  stand-alone  mode,  none  of  the  existing  navigation 
technologies  has  a  potential  to  satisfy  the  requirements  for  the  navigation  accuracy,  continuity  and  availability 
over  the  entire  duration  of  the  UAV  flight.  Therefore,  multi-sensor  fusion  techniques  are  pursued.  In  other 
words,  to  be  able  to  navigate  at  any  environment  at  any  time,  it  is  beneficial  to  utilize  any  potential  source  of 
navigation  related  information. 

Example  applications  that  involve  navigation  in  difficult  environments  include  but  are  not  limited  to 
navigation,  guidance  and  control  of  autonomous  ground  vehicle  (UGV)  and  autonomous  aerial  vehicle 
(UAV),  as  well  as  teams  of  UGVs  and  UAVs  for  urban  surveillance  and  reconnaissance  tasks;  geographical 
information  system  (GIS)  data  collection  for  mapping  applications  on  open  highways  and  dense  urban 
environments;  indoor  search  and  rescue  applications;  monitoring  of  urban  infrastructure  for  situational 
awareness;  and,  precise  automotive  applications  such  as  automated  lane  keeping.  Meter-level  to  decimeter- 
level  reliable  positioning  capabilities  are  generally  needed  for  these  application  examples.  As  stated 
previously,  none  of  the  existing  navigation  technologies  can  currently  satisfy  these  requirements  or  has  a 
potential  to  provide  these  capabilities  in  a  stand-alone  mode. 

This  paper  discusses  multi-sensor  fusion  approaches  for  navigation  in  difficult  environments.  A  generic 
concept  of  the  multi-sensor  fusion  is  first  presented.  Next,  the  paper  exemplifies  applications  of  the  generic 
multi-sensor  fusion  concept  for  the  development  of  specific  multi-sensor  mechanizations.  Specifically, 
integrated  Global  Positioning  System  (GPS)/laser  scanner/INS  and  laser  scanner/video  camera/INS 
mechanizations  are  considered. 
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2.0  MULTI-SENSOR  FUSION  APPROACH 

The  generic  concept  of  the  multi-sensor  navigation  utilizes  a  self-contained  inertial  navigator  as  a  core 
navigation  sensor.  The  INS  does  not  rely  on  any  type  of  external  information  and  as  a  result  can  operate  in  any 
environment.  However,  inertial  navigation  solution  drifts  over  time  [1],  To  mitigate  INS  drift,  this  core  sensor 
is  augmented  by  reference  navigation  data  sources  (such  as,  for  example,  GPS  or  a  laser  scanner).  Reference 
data  sources  generally  rely  on  external  observations  or  signals  that  may  or  may  not  be  available.  Therefore, 
these  sources  are  treated  as  secondary  sensors.  When  available,  secondary  sensors’  measurements  are  applied 
to  reduce  the  drift  in  inertial  navigation  outputs.  Inertial  data  are  used  to  bridge  over  reference  sensor  outages. 
In  addition,  inertial  data  can  be  applied  to  improve  the  robustness  of  reference  sensor  signal  processing:  for 
instance,  to  significantly  increase  the  GPS  signal  integration  interval  in  order  to  enable  processing  of  very 
weak  GPS  signals  and  to  reduce  the  susceptibility  of  GPS  to  radio-frequency  interference  [2],  Figure  2 
illustrates  the  multi-sensor  fusion  approach. 


Figure  2.  Generic  multi-sensor  fusion  approach 


Figure  3  provides  a  more  detailed  illustration  of  the  interaction  between  the  INS  and  a  secondary  navigation 
sensor. 


Figure  3.  Data  fusion  between  the  INS  and  a  secondary  navigation  sensor 
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The  secondary  sensor  generally  includes  a  signal  processing  part  and  a  navigation  solution  part.  The  signal 
processing  part  receives  navigation-related  signals  and  measures  their  parameters.  For  example,  GPS  receiver 
tracking  loops  [3]  or  open- loop  batch  estimators  [4]  measure  parameters  (pseudoranges,  Doppler  frequency 
shift,  and  carrier  phase)  of  the  received  GPS  signals.  Another  example  is  a  laser  scanner  time-of-flight 
measurement  that  is  directly  related  to  the  distance  between  the  scanner  and  a  reflecting  object.  Measurements 
of  signal  parameters  are  then  applied  to  compute  the  navigation  solution.  For  example,  GPS  pseudoranges  are 
used  to  compute  the  GPS  receiver  position  [3],  changes  in  distances  to  reflecting  stationary  objects  are 
exploited  to  compute  the  change  in  the  position  of  the  laser  scanner  [5].  Note  that  the  navigation  solution  can 
only  be  computed  if  a  sufficient  number  of  signal  measurements  is  made.  For  example,  at  least  four 
pseudorange  measurements  must  be  available  to  compute  the  GPS-based  position;  at  least  two  lines  must  be 
extracted  from  an  image  of  a  two-dimensional  (2D)  laser  scanner  to  compute  a  2D  laser  position  [5]. 

The  multi-sensor  fusion  architecture  above  applies  secondary  sensor’s  signal  measurements  to  estimate  drifts 
in  inertial  navigation  outputs.  This  approach  is  generally  referred  to  as  tight  coupling.  The  use  of  signal 
measurements  for  the  INS  drift  re-calibration  is  more  beneficial  as  compared  to  the  use  of  the  secondary 
sensor’s  navigation  solution  (this  form  of  sensor  fusion  is  referred  to  as  loose  coupling).  As  opposed  to  loose 
coupling,  tight  coupling  still  allows  for  the  INS  drift  mitigation  even  for  those  cases  where  insufficient 
number  of  signal  measurements  is  available  and  the  complete  navigation  solution  cannot  be  derived. 

The  estimation  of  the  INS  drift  terms  is  performed  using  the  mechanism  of  a  complementary  Kalman  filter. 
The  idea  is  that  a  signal  parameter  can  be  generally  represented  as  a  function  of  position,  velocity  and  attitude. 
This  function  is  computed  based  on  INS  navigation  outputs  and  then  compared  to  the  actual  signal 
measurement.  A  discrepancy  between  the  INS-based  signal  prediction  and  the  signal  measurement  is  used  by 
the  complementary  Kalman  filter  mechanization  to  estimate  INS  drift  terms.  A  generic  Kalman  filter 
measurement  observable  y  is  formulated  as  follows: 

y  =  P(RINS’VINS>aiNs)“P  (1) 

where: 

R1NS  is  the  INS  position  vector; 

VINS  is  the  INS  velocity  vector; 

aINS  is  the  INS  attitude  (that  can  be  represented  as  a  direction  cosine  matrix,  rotation  vector,  rotation 
quaternion,  or  Euler  angles  [1]); 

p(R1NS,  VINS,aINS)  is  the  signal  value  predicted  based  on  the  inertial  output;  and, 
p  is  the  actual  signal  measurement. 

The  signal  measurement  herein  is  denoted  by  p  since  the  tight  coupling  approach  was  first  applied  to  the 
GPS/INS  integration  case  in  order  to  fiise  GPS  pseudorange  measurements  and  integrated  Doppler  range 
measurements  with  inertial  data  [6],  For  this  reason,  the  INS  calibration  in  the  signal  measurements’  domain 
is  also  often  referred  to  as  the  range-domain  data  fusion.  Obviously,  the  formulation  above  is  not  limited  to 
the  GPS/INS  integration  case  and  is  applicable  to  any  type  of  navigation-related  signal  measurements.  It  is 
important  to  mention  that  video  measurements  serve  as  an  exclusion  for  this  general  rule  of  the  observation 
formulation.  For  a  vision-based  case  where  the  distance  to  a  navigation-related  feature  is  unknown  and  cannot 
be  measured  (for  example,  a  monocular  camera  case  without  any  prior  feature  information),  image  features 
are  related  to  navigation  parameters  not  in  the  form  of  a  non-linear  function,  but  rather  in  the  form  of  a  non¬ 
linear  motion  constraint.  This  constraint  is  formulated  as  f(R,a,m,p)  =  0 ,  where  m  represents  feature 
homogeneous  coordinates  (i.e.,  Cartesian  coordinates  scaled  by  the  image  depth)  and  p  is  the  distance  to  the 
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feature.  In  this  case,  non-linear  motion  constrains  and  inertial  data  are  used  to  formulate  Kalman  filter 
observables:  i.e.,  f(RINS,aINS,m,p)  =  0,  where  m  denotes  homogeneous  feature  coordinates  that  are  extracted 
from  video  images. 


To  implement  a  complementary  Kalman  filter,  equation  (1)  is  linearized  as  using  a  Taylor  series  expension: 


y  -  p(RINS,  VINS,aINS)  p 


P+^6R 
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.  dp  dp 

INS  ^  UIVINS  "r 
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where: 

p  is  the  true  value  of  the  signal  parameter; 
np  is  the  measurement  noise;  and, 

5Rins  ,  8VINS ,  and  8a1NS  are  the  INS  position,  velocity  and  attitude  errors,  respectively. 


(2) 


Non-linear  motion  constraints  are  linearized  for  the  case  of  video  measurements.  These  linearized 
formulations  are  then  utilized  by  standard  Kalman  filter  routines  (i.e.,  prediction,  estimation  and  covariance 
updates)  to  estimate  INS  error  states. 

For  those  cases  where  multiple  measurements  are  available  from  secondary  navigation  sensors,  the 
measurement  observation  vector  is  expressed  as  follows: 

{y /  * }=  Pi  ^(®-iNS’^iNS’aiNs)  — P/ 

1  =  1,..., L  (3) 

i  =  l,...,/ 

In  (3),  /  is  the  reference  sensor  index  and  i  is  the  measurement  index  for  a  particular  reference  sensor.  For 
example,  reference  sensors  can  include  GPS,  laser  scanner  and  Doppler  radar  with  corresponding  indexes  1,  2, 
and  3.  In  this  case,  y)51  represents  a  5  th  measurement  observable  for  laser  scanner  measurements.  Specific 
formulations  of  Kalman  filter  measurement  observations  are  exemplified  in  Section  3  for  cases  of  GPS,  laser 
scanner  and  vision  reference  sensors. 

As  mentioned  previously,  while  the  secondary  sensors’  measurements  are  used  to  improve  the  inertial 
accuracy,  the  INS  data  can  be  applied  to  improve  the  robustness  of  the  secondary  sensor’s  signal  processing. 
For  example,  inertial  data  can  be  used  for  robust  matching  of  features  between  different  images  of  a  video 
camera  or  a  laser  scanner.  Another  example  is  the  use  of  inertial  aiding  for  GPS  signal  integration  in  order  to 
enable  tracking  of  signals  that  are  attenuated  by  buildings  [7]. 

Secondary  navigation  sensors  that  can  be  applied  for  the  multi-sensor  fusion  with  the  INS  include: 

GNSS  and  partial  GNSS:  GPS  and  GNSS  signal  measurements  can  be  efficiently  integrated  with 
inertial  data  in  open  areas  and  also  in  difficult  GPS  signal  environments  (such  as  urban  areas)  where 
limited  GNSS  signals  may  still  be  available  [7],  [8]. 
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Feature-based  navigation  sensors:  Examples  include  image-aided  inertial  navigation  [9]  and  ladar- 
aided  inertial  navigation  [5]. 

Beacon-based  navigation  (including  pseudolites):  If  the  GPS  signal  is  not  adequate  for  navigation  in  a 
particular  environment,  it  is  possible  to  transmit  an  additional  signal  or  signals  that  are  specifically 
designed  for  navigation  purposes.  If  the  transmitted  signals  are  similar  to  GPS  signals,  then  such  beacon 
transmitters  are  usually  called  “pseudolites.”  Examples  of  beacon-based  navigation  systems  for  indoor 
navigation  can  be  found  in  [10]  and  [11]. 

Signals  of  opportunity  (SoOP).  Signals  of  opportunity,  as  defined  in  this  paper,  are  radio  frequency 
(RF)  signals  that  are  not  intended  for  navigation.  Examples  from  previous  research  include  digital 
television  [12],  analog  television[13],  and  AM  radio  [14],  [15]. 

The  remainder  of  this  paper  illustrates  application  of  the  generic  multi-sensor  fusion  approach  described 
above  GPS/laser/inertial  and  vision/laser/inertial  integrated  mechanizations. 


3.0  EXAMPLE  MULTI-SENSOR  FUSION  MECHANIZATIONS 
3.1  GPS/Laser  Scanner/Inertial 

Feature -based  navigation  techniques  represent  a  viable  option  for  navigation  in  difficult  GPS  environments. 
For  example,  the  integrated  laser  scanner/INS  solution  was  demonstrated  to  provide  sub-meter  accurate 
navigation  for  dense  urban  environments  where  multiple  lines  can  be  extracted  from  scan  images  [5]. 
However,  the  feature-based  navigation  approach  clearly  has  its  limitations.  Relatively  open  streets  represent 
challenging  conditions  for  the  line -based  navigation  due  to  limited  line  availability.  To  illustrate,  Figure  4 
shows  a  scan  image  recorded  on  a  relatively  open  street.  For  this  example,  only  one  line  is  extracted  from  the 
image  and  the  system  must  augment  its  position  computations  by  the  INS  coasting  option. 


Figure  4.  Example  of  the  laser  scan  image  taken  on  a  relatively  open  street:  only  one  line 
is  extracted  from  the  image,  which  is  insufficient  to  compute  the  position  solution 

In  addition,  even  in  dense  urban  canyons,  line  geometry  observed  in  scan  images  can  be  insufficient  to  support 
complete  observability  of  the  navigation  states. 

Figure  5  illustrates  the  case  where  lines  in  the  scan  image  are  created  by  two  nearly  parallel  building  walls.  In 
this  case,  lines  extracted  from  laser  scans  are  nearly  collinear.  This  line  geometry  allows  for  the  estimation  of 
the  cross-track  position  component  only:  i.e.,  the  vehicle  motion  component  in  the  direction  perpendicular  to 
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the  walls  can  be  estimated,  while  the  along-track  (parallel  to  the  walls)  motion  component  is  unobservable  and 
INS  coasting  has  to  be  used  to  compute  vehicle  displacement  in  this  direction. 


Figure  5.  Example  of  the  laser  scan  in  an  urban  canyon:  nearly  collinear 
lines  are  extracted,  which  limits  the  observability  of  position  states 


The  use  of  GPS  can  efficiently  augment  the  feature-based  navigation  approach.  Relatively  open  streets  (see, 
for  example,  Figure  4)  generally  provide  enough  open  sky  visibility  to  track  a  number  of  GPS  satellite  signals 
that  is  sufficient  for  navigation  computations.  In  dense  urban  canyons  where  only  limited  feature  geometry  is 
available  (see,  for  example,  Figure  5),  a  reduced  number  of  satellite  signals  can  be  still  trackable  through 
limited  portions  of  an  open  sky.  Combining  line  measurements  with  these  additional  GPS  measurements  can 
complete  the  observability  of  navigation  states.  To  exemplify,  the  availability  of  high-elevation  satellites  that 
have  line-of-sight  azimuth  angles  aligned  with  the  direction  of  an  urban  canyon  shown  in  Figure  5  can 
complete  the  observability  for  the  along-track  position  component.  In  this  case,  two  satellites  are  required  to 
complete  the  2D  position  and  clock  solution.  One  satellite  is  sufficient  for  the  case  where  a  reliable  estimate  of 
the  GPS  receiver  clock  bias  is  available:  i.e.  the  receiver  clock  bias  and  clock  drift  have  been  previously 
estimated  and  the  clock  stability  figures  allow  for  an  accurate  propagation  of  the  clock  state  estimates  from  the 
estimation  time  to  the  current  measurement  epoch. 

Integration  of  GPS,  laser  scanner  and  inertial  data  is  discussed  in  the  remainder  of  this  subsection.  Figure  6 
illustrates  the  integrated  system  architecture. 


RTO-EN-SET-1 16(2010) 


9-7 


Navigation  in  Difficult 

Environments:  Multi-Sensor  Fusion  Techniques 


ORGANIZATION 


Figure  6.  Integrated  GPS/laser  scanner/INS  system  architecture 


This  architecture  combines  GPS,  laser  scanner,  and  inertial  data  for  trajectory  reconstruction,  i.e.  the 
estimation  of  changes  in  the  user  position  (or  delta  position)  between  successive  GPS  and  laser  measurement 
epochs.  An  open  loop  software  GPS  receiver  [4]  is  exploited  for  robust  carrier  phase  tracking  in  difficult 
environments.  The  integrated  solution  utilizes  INS  navigation  outputs  to  improve  the  solution  robustness. 
Particularly,  inertial  data  are  applied  to  1)  predict  feature  displacements  between  laser  scans  for  robust  feature 
association  between  scan  images,  and,  2)  computationally  adjust  a  2D  scan  plane  for  tilting  of  the  laser 
scanner  platform. 

As  shown  in  Figure  6,  a  Kalman  filter  is  used  to  periodically  compute  estimates  of  inertial  error  states  that  are 
then  applied  to  mitigate  the  drift  in  INS  navigation  outputs.  Kalman  filter  observables  are  formulated  in  the 
measurement  domain  of  GPS  and  laser  scanner,  which  is,  as  mentioned  previously,  referred  to  as  range- 
domain  data  fusion.  Specifically,  changes  in  GPS  carrier  phase  measurements  and  changes  in  ranges  to  lines 
extracted  from  scan  images  serve  as  filter  measurement  observables.  A  complementary  filter  formulation  [16] 
is  utilized  for  the  efficient  linearization  of  system  state  relations:  i.e.,  filter  measurement  observables  are 
computed  as  differences  between  GPS  (and/or  laser  scanner)  measurement  observables  and  INS  navigation 
outputs  transformed  into  the  GPS  (and/or  laser  scanner)  measurement  domain.  In  addition,  a  dynamic-state 
filtering  approach  [17]  is  exploited.  In  this  case,  the  filter  implements  displacement  error  states  (dynamic 
states)  rather  than  absolute  position  error  states. 

GPS  observables  of  the  Kalman  filter  are  based  on  carrier  phase  single  differences  (SDs)  between  the  filter 
update  epochs.  The  SD  approach  allows  exploiting  mm-level  accurate  carrier  phase  measurements  for 
trajectory  reconstruction  without  the  need  to  resolve  integer  ambiguities.  Equation  (4)  formulates  the  carrier 
phase  SD  equation  [18]: 

A$J  =  9j  (tM )  -  9j  (t  M-1 )  =  Ari  +  A§trcvr  +  Errors  j  +  Apj  (4) 
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where: 

Atp  j  is  the  carrier  phase  SD  for  satellite  j; 

cp .  is  the  carrier  phase  measurement  for  satellite  j; 

tM  =  t0  +  M  •  AtGPS  is  the  discrete  time  and  AtGPS  is  the  GPS  measurement  update  interval; 

Arj  =  rj  (tM )  —  r j (tM_j )  is  the  SD  of  the  range  r,  between  the  GPS  antenna  and  satellite  j; 

A5trcvr  is  the  SD  of  the  receiver  clock  bias,  or,  equivalently,  the  receiver  clock  drift  accumulated  over  the 
AtOPS  interval; 

the  AerrorS|  term  represents  changes  in  deterministic  error  components  of  stand-alone  GPS  measurements 

and  includes  changes  in  ionospheric  and  tropospheric  delays,  changes  in  the  satellite  clock  bias,  and  drift 
components  of  relativistic  corrections;  and, 

Ar| j  is  the  joint  noise  and  multipath  term,  which  includes  carrier  noise  and  multipath. 

The  SD  in  satellite/receiver  range  is  expressed  as  follows: 

Arj  =  SV  Dopple^  -  Ageometryj  -  (ej(tM), AR)  (5) 

where: 

SV  Doppler^  is  a  change  in  the  range  due  to  the  satellite  motion  along  the  line-of-sight  (LOS); 

Ageometryj  accounts  for  changes  in  the  relative  satellite/receiver  geometry; 

e  j  is  the  unit  vector  pointed  from  the  receiver  to  the  satellite,  this  vector  is  generally  referred  to  as  the  LOS 
unit  vector; 

AR  is  the  receiver  position  change  vector  for  the  interval  [tM-i,tM];  and, 

(,)  is  the  vector  dot  product. 

Carrier  phase  SDs  are  adjusted  for  the  satellite  motion  terms,  geometry  terms,  and  delta  error  terms  prior  to 
their  exploitation  as  Kalman  filter  measurement  observables.  For  the  SD  adjustment,  satellite  motion  and 
geometry  terms  are  computed  as  follows  [18]: 

SV  Dopple^  =  (ej(tM),Rsvj(tM))  -  (ej(tM_,),Rsvj(tM_1))  (6) 

Ageometryj  =  (ej(tM)-  ej(tM_1),R(tM_1))  (7) 

In  (6)  and  (7): 

Rsvj  is  the  satellite  position  vector;  and, 

R  is  the  receiver  position  vector. 

For  geometry  compensation,  the  receiver  position  vector  R  at  the  previous  update  (tM-i)  is  estimated  based  on 
GPS  pseudorange  measurements.  For  those  cases  where  not  enough  pseudorange  measurements  are  available, 
the  position  estimate  is  propagated  using  inertial  data.  Note  that  a  sub-hundred-m  level  accurate  position 
estimate  is  generally  sufficient  to  support  mm-level  accuracy  in  the  carrier  phase  SDs  [18].  The  satellite 
position  Rsvj  vector  is  computed  from  ephemeris  data,  and  the  LOS  unit  vector  e  is  computed  based  on 

ephemeris  data  and  the  pseudorange-based  receiver  position  estimate.  Tropospheric  drift  terms  are 
compensated  based  on  tropo  models  [3].  Iono  delta  errors  are  normally  compensated  using  dual  frequency 
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measurements  [3].  However,  generally,  iono  drift  terms  stay  at  a  mm/s  level  or  less  unless  ionospheric 
scintillations  are  present  [18].  Thus,  for  most  operational  scenarios,  uncompensated  iono  drift  does  not 
significantly  influence  the  accuracy  of  carrier  phase  SDs.  For  this  reason,  the  system  mechanization  reported 
in  this  paper  does  not  implement  iono  corrections. 

From  equation  (4)  and  (5),  carrier  phase  SDs  that  are  adjusted  for  the  satellite  motion,  geometry  changes,  and 
delta  error  terms  are  expressed  as  follows: 

Acp  jdj(tM)  =  -(ej(tM),  AR)+  AStrcvr  +  An  j  (8) 


GPS  observables  of  the  Kalman  filter  are  computed  as  differences  between  INS-based  predicted  values  of 
carrier  phase  SDs  and  carrier  phase  SDs  that  are  actually  measured  by  the  GPS  receiver.  For  satellite  j,  this 
difference  is  formulated  as: 


^Kalman  =A~INS_A~adj  (9) 

where: 

Acp'NS  =-(ej(tM),ARms  +  AC?  -L^)  (10) 

In  (10),  AC^  =C^(tM)-Cb  (tM_j)  and  L^u  is  the  lever-arm  vector  between  the  inertial  measurement  unit 
(IMU)  and  GPS  antenna  with  the  lever  arm  vector  components  being  resolved  in  the  body  frame.  Equations 
(9)  and  (10)  illustrate  the  specific  implementation  of  the  generic  multi-sensor  fusion  observable,  which  is 
formulated  by  equation  (1),  for  the  case  where  GPS  carrier  phase  measurements  serve  as  reference 
measurements  for  the  INS  drift  estimation. 

Laser  scanner  observables  of  the  Kalman  filter  are  computed  from  navigation  related  features  observed  in  scan 
images.  These  observations  are  illustrated  in  Figure  7. 


Figure  7.  Laser  scanner  motion  and  observed  change  in  line  parameters 

For  the  laser  scanner  case,  lines  are  chosen  as  the  basis  navigation  feature  [5].  Kalman  filter  observables  are 
formulated  based  on  changes  in  line  parameters  between  consecutive  scans.  As  illustrated  in  Figure  7,  changes 
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in  the  scanner  location  between  scans  create  changes  in  parameters  of  lines  observed  in  scan  images.  In  Figure 
7,  line  is  represented  by  its  normal  point,  where  a  normal  point  is  defined  as  a  perpendicular  intersection  of  the 
line  itself  and  the  line  originating  from  the  scanner  location.  Line  normal  points  are  characterized  by  their 
polar  parameters:  range  p  and  angle  a.  From  the  geometry  shown  in  Figure  7,  position  change  is  related  to  the 
line  range  change  as  follows  [5]: 


APk=Pk(tM)-Pk(tM-i)  =  -ARxCOs(ak(tM_1)yARYsin(ak(tM_1)yA8k  (11) 

where: 

Apk  is  the  line  range  change  for  line  k; 

pk  is  the  line  range  estimated  by  a  line  extraction  procedure  (e.g.,  using  a  modified  iterative  split  and 
merge  line  extraction  algorithm  described  in  reference  [19]); 

ARX  and  ARY  are  position  change  components; 

ak  is  the  line  angle;  and, 

Aek  is  the  noise  in  estimated  line  delta  range  that  is  due  to  line  extraction  errors;  these  errors  generally 
comprise  of  laser  measurement  noise  and  a  texture  of  a  scanned  surface. 

Note  that  equation  (11)  operates  with  line  parameters  that  are  transformed  into  the  horizontal  plane  using  the 
laser  tilt  compensation  procedure  described  in  [5].  Lines  are  observed  at  the  laser  scanner  body  frame.  This 
frame  can  be  tilted  due  to  non-zero  pitch  and  roll  angles  of  the  scanner  platform.  The  tilt  compensation 
procedure  exploits  INS  attitude  outputs  to  project  lines  observed  in  the  scanner  body-frame  onto  a  horizontal 
plane  of  the  navigation  frame  in  order  to  mitigate  the  influence  of  laser  tilt  angles  on  the  navigation  solution 
accuracy.  Thus,  equation  (11)  relates  changes  in  line  parameters  with  changes  in  position  vector  components 
that  are  resolved  in  the  axes  of  the  navigation  frame.  For  the  system  implementation  considered  in  this  paper, 
navigation  is  performed  at  the  East-North-Up  (ENU)  frame.  Laser  scanner  body-frame  and  body-frame  of  the 
inertial  measurement  unit  (IMU)  are  computationally  aligned  to  the  axes  of  the  navigation  frame  during  the 
system  initialization  stage.  IMU  body-frame  is  physically  aligned  with  the  laser  body-frame  and  misalignment 
errors  are  calibrated  during  the  system  initialization. 

Below,  equation  (1 1)  is  reformulated  in  a  vector  form: 

APk  =-(nk(tMXAR)  +  Aek  (12) 

where: 

nk(tM-i)=[cos(ak(tM_,))  sin(ak(tM_,))  o]  (13) 


A  laser  scanner  observable  of  the  Kalman  filter  is  computed  as  a  difference  between  line  delta  range  predicted 
based  on  inertial  data  and  a  line  delta  range  extracted  from  scanner  measurements.  For  line  p,  this  is 
formalized  as  follows: 


where: 


Kalman 

Vk 
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(14) 


apIns 


-(nk(t 


m)’ARINS  +  AC-b 


N  t  Laser' 
‘^IMU 


(15) 


RTO-EN-SET-1 16(2010) 


9-11 


Navigation  in  Difficult 

Environments:  Multi-Sensor  Fusion  Techniques 


ORGANIZATION 


and,  is  the  IMU  to  laser  scanner  lever  arm  resolved  in  the  body-frame.  Note  that  equations  (14)  and  (15) 
represent  a  specific  implementation  of  the  generic  multi-sensor  fusion  observable  defined  by  equation  (1)  for 
the  case  of  laser/inertial  data  fusion. 


GPS  and  laser  observables  of  the  Kalman  filter  are  combined  into  a  joint  filter  measurement  vector: 


_  ["  Kalman  Kalman  Kalman 

Kalman  “  [^1  UJ  V1 


Kalman  T 

VK  J 


(16) 


As  mentioned  previously  the  Kalman  filter  implements  the  dynamic-state  estimation  approach:  i.e.,  the  filter 
does  not  estimate  absolute  position;  instead,  position  changes  between  GPS  and  laser  scanner  measurement 
updates  are  estimated.  The  state  vector  for  the  dynamic-state  filter  formulation  includes  twenty  states:  delta 
position  error  states  (three  states);  velocity  error  states  (three  states);  attitude  error  states  (three  states);  delta 
attitude  error  states  (three  states)  -  these  are  attitude  errors  accumulated  over  the  filter  update  interval;  gyro 
bias  states  (three  states);  accelerometer  bias  states  (three  states);  and,  GPS  receiver  clock  states  that  include 
delta  bias  state  and  drift  state.  For  this  state  vector,  the  filter  observation  matrix  (H^iman)  is  derived  from  the 
filter  observation  equations  (9),  (10),  (14)  and  (15).  Elements  of  this  matrix  projections  of  filter  states  into  the 
filter  observation  domain  as  illustrated  in  Figure  8. 
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Figure  8.  Kalman  filter  observation  matrix:  matrix  elements  define  projection 
coefficients  for  projecting  filter  states  into  the  filter  observation  domain 


In  Figure  8: 

0’s  are  3x1  zero  rows; 

and  a,  b,  c,  d  are  3x1  rows  that  account  for  the  transformation  of  INS  attitude  error  terms  into  the  filter 
observables  through  the  lever-arm  compensation. 

These  matrices  were  derived  using  the  approach  proposed  in  [17].  Results  of  the  derivation  are  as  follows: 


9  - 12 


RTO-EN-SET-1 16(2010) 


5Jg 


Navigation  in  Difficult 
Environments:  Multi-Sensor  Fusion  Techniques 


a,  =  ( 

qx(AC*- 

Lgps 

Mmu  JJ 

bH 

fjX^tM.O-L®*))1 

cH 

Z  X) 

<<Ls 

X 

44 

v  S3  y 

j  Laser 
*  ^IMU  JJ 

nkx(C*(t 

\  t  Laser 

M-l/  '^IMU  II 

(17) 


where: 

x  is  the  vector  dot  product. 


The  filter  measurement  noise  matrix  is  a  diagonal  matrix  defined  by  variances  in  carrier  phase  SDs  and  line 
delta  ranges: 
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Carrier  phase  sigmas  (aA  )  are  computed  as  follows: 


aA(pj  -  a(pj  (f  M  )  +  CTpj  (t-M-1 ) 
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where: 

Ll1  is  wavelength  of  the  GPS  link  1  (LI)  carrier  (0.19  cm,  approximately); 

Tint  is  the  GPS  receiver  signal  integration  interval;  and, 

C/No  is  the  carrier-to-noise  ratio  that  is  routinely  estimated  by  the  open  loop  GPS  receiver. 

Laser  delta  range  sigmas  (crAPk )  are  calculated  based  on  equation  (20): 

aApk(tM)  =  apk(tM)  +  apk(tM-l)  (20) 

k  =  1,...,K 

where: 

CTAp!  are  line  range  sigmas  that  are  estimated  using  the  approach  reported  in  [20]. 

With  the  filter  states  listed  above,  filter  measurements  formulated  by  equations  (9),  (10),  (14)  and  (15),  and  filter 
matrices  represented  in  Figure  8  and  defined  by  equations  (17)  and  (18),  the  INS  drift  estimation  procedure 
implements  a  complementary  Kalman  filter  algorithm  for  the  estimation  of  the  inertial  error  states  and  GPS 
receiver  clock  states.  The  filter  formulation  is  similar  to  the  GPS/INS  Kalman  filter  model  found  in  [16]. 

Performance  of  the  GPS/laser  scanner/inertial  solution  is  illustrated  below  using  experimental  data  collected 
in  real  urban  environments.  A  test  van  was  used  as  a  platform  for  urban  data  acquisition.  The  data  were 
acquired  in  Athens,  Ohio,  USA.  Figure  9  shows  a  photograph  of  the  data  collection  setup. 
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The  setup  used  to  acquire  and  process  live  GPS,  laser  scanner,  and  inertial  urban  data  includes: 

•  Laser  scanner:  SICK  LMS-200.  A  centimeter  distance  measurement  mode  was  chosen.  For  this  mode, 
a  standard  deviation  of  the  laser  ranging  noise  is  specified  as  5  mm.  The  maximum  measurement 
range  is  80  m.  A  scan  angular  range  is  from  0  to  180  deg  with  an  angular  resolution  of  0.5  deg.  A 
scanner  update  rate  of  one  scan  per  0.4  s  was  used. 

•  IMU:  Systron  Donner  DQI  IMU.  This  IMU  represents  a  tactical-grade  unit  whose  main 
characteristics  are  summarized  in  Table  1. 


Laser  scanner 


IMU 


Laser/IMU 
synchronization  and 
data  collection  board 


Software 

receiver 

front-end 


Figure  9.  Data  collection  setup  used  for  the  acquisition  of  GPS/Laser  scanner/inertial  urban  test  data 


Table  1-IMU  characteristics 


Parameter 

Value 

Gyro  bias  in-run  stability 

3  deg/hr  (sigma) 

Gyro  noise 

0.035  deg/Vhr  (sigma) 

Gyro  scale  factor 

350  ppm  (sigma) 

Accelerometer  bias  in-run  stability 

200  pg  (sigma) 

Accelerometer  noise 

200  pg/ Vtfz  (sigma) 

Accelerometer  scale-factor 

350  ppm  (sigma) 

Sensor  axis  non-orthogonality 

0.5  mrad 

•  GPS  receivers:  A  software  receiver  front-end  developed  at  the  Ohio  University  Avionics  Engineering 
Center  was  used  for  collection  of  raw  GPS  signal  samples  [21].  A  NovAtel  Superstar  II  receiver 
provided  1  PPS  signal  for  time-stamping  of  laser  scanner  and  inertial  measurements. 

•  Synchronization  and  data  collection  board:  Xilinx  Spartan-3  Field  Programmable  Gate  Arrays 
(FPGA).  The  board  decodes  laser  and  inertial  data  from  corresponding  measurement  sensors,  time 
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stamps  the  measurements  decoded  using  1  PPS  signal,  and  then  sends  time-stamped  measurements 
via  a  Digilent  USB  board  to  a  PC  that  collects  the  data  for  post-processing. 

•  Data  processing:  Post-processing  implemented  in  Matlab™. 

Figure  10  shows  test  trajectory  implemented  for  the  first  test  scenario.  This  figure  shows  the  vehicle  trajectory 
along  with  images  of  the  environment  taken  at  select  trajectory  points.  The  test  trajectory  shown  is 
reconstructed  by  the  laser/INS  integration  that  is  described  in  [5]. 


vehicle  path 


Figure  10.  Test  trajectory:  trajectory  reconstructed  by  the  laser/INS  integration  is 
shown  along  with  images  of  the  environment  taken  at  select  trajectory  points 


As  it  can  be  seen  from  the  images  represented,  the  test  trajectory  includes  both  relatively  open  streets  as  well 
as  dense  urban  canyons.  Thus,  this  trajectory  allows  testing  performance  of  the  integrated  solution  in  a  variety 
of  urban  environmental  conditions. 

Figure  1 1  illustrates  complementary  availability  of  GPS  and  laser  measurements  for  the  first  test  scenario. 
Availability  of  GPS  measurements  is  represented  by  satellite  tracking  statuses.  A  particular  satellite  is 
designated  by  its  pseudorandom  code  number  (PRN).  The  satellite  tracking  status  is  shown  using  a  black-to- 
white  color  scheme.  Black  color  indicates  satellites  with  a  strong  C/No  (50  dB-Flz  or  higher).  White  color  is 
used  for  satellites  with  a  C/No  value  below  the  tracking  threshold  of  32  dB-Flz:  i.e.  white  color  indicates  that  a 
satellite  is  not  visible.  The  black-to-white  color  scheme  is  also  applied  to  illustrate  the  availability  of  features 
extracted  from  scan  images.  Black  color  indicates  that  multiple  lines  are  extracted  from  scan  images  and  the 
line  geometry  allows  for  complete  navigation  computations.  White  color  designates  cases  where  no  lines  are 
extracted.  As  shown  in  Figure  11,  GPS  and  laser  scanner  measurements  exhibit  complementary  availability. 
Limited  or  no  GPS  measurements  are  available  in  dense  canyons  where  multiple  lines  are  extracted  from  scan 
images:  see,  for  example,  a  portion  of  the  plot  that  corresponds  to  the  urban  canyon  image.  Vice-versa,  a 
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sufficient  number  of  visible  satellites  is  present  on  open  streets  where  limited  or  no  lines  are  extracted  from 
laser  scans:  see,  for  example,  a  portion  of  the  plot  that  corresponds  to  the  open  street  image. 

Delta  position  residuals  are  applied  to  characterize  the  trajectory  estimation  performance.  Residual  vector 
( 8R )  is  computed  as  the  difference  between  the  laser/GPS  weighted  LMS  estimate  of  the  delta  position 
( ARLaser/GPS)  and  inertial  delta  position  (AR1NS)  that  is  compensated  for  drift  terms  using  Kalman  filter 
predictions: 


SR  =  AR  Laser /GPS  "  ARjns  ~  ^u) ~  ^  (W))  L®* 


(21) 


Satellite  tracking  status 


C/No,  dB-Hz 
50 


15  Number  of 
lines  extracted 


Figure  11.  Complementary  availability  of  GPS  and  laser  scanner  measurements 


The  GPS/Laser  LMS  delta  position  solution  is  computed  based  on  GPS  carrier  phase  SDs  and  laser  line  delta 
ranges  without  the  use  of  the  inertial.  LMS  estimation  details  are  given  in  [8].  Essentially,  delta  position 
residuals  characterize  the  level  of  noise  in  the  reconstructed  trajectory.  This  noise  is  a  combined  effect  of  the 
GPS  carrier  phase  noise,  noise  in  line  ranges  and  the  noise  component  of  INS  position  drift.  Figure  12  shows 
residual  plots  for  East  and  North  components  of  delta  position. 
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Figure  12.  Delta  position  residuals  for  the  GPS/laser/INS  urban  test  example 


The  standard  deviations  of  East  and  North  residual  components  are  computed  as  3.0  cm  and  2.0  cm, 
respectively.  For  a  similar  test  trajectory,  residual  standard  deviations  of  the  laser/INS  integrated 
implementation  (i.e.,  when  GPS  observables  of  the  Kalman  filter  are  not  used)  are  at  a  7-cm  level  [5]. 
GPS/laser/INS  residual  noise  is  generally  increased  when  the  LMS  solution  relies  primarily  on  line  ranges  as 
compared  to  cases  with  good  satellite  availability.  This  is  due  to  a  higher  level  of  noise  in  line  ranges  (caused 
mostly  by  the  texture  of  scanned  urban  surfaces)  as  compared  to  the  carrier  phase  noise.  For  example,  an 
increased  residual  noise  can  be  observed  for  the  time  interval  starting  at  approximately  60  s  and  ending  at 
approximately  130  s.  This  interval  corresponds  to  a  part  of  the  trajectory  that  belongs  to  dense  urban 
enviromnents  where  limited  satellites  are  available:  for  illustration  of  the  environment,  see  the  third  image 
from  the  trajectory  start  in  Figure  10. 

3.2  Vision/Laser  Scanner/Inertial  Integration 

Vision-based  navigation  techniques  serve  as  a  viable  option  for  autonomous,  passive  navigation  and  guidance 
in  Global  Positioning  System  (GPS)-denied  environments  [9].  However,  vision-based  methods,  including 
stereo-vision,  are  known  to  be  brittle  to  signal  noise,  particularly  in  terms  of  estimating  the  image  depth. 
Stereo  vision  suffers  from  several  disadvantages.  The  first  is  synchronization:  if  the  stereo  system  is  moving, 
and  the  two  images  are  not  captured  at  the  same  time,  the  time  discrepancy  between  the  two  images  can 
invalidate  the  stereo  reconstruction.  This  timing  discrepancy  cannot  be  completely  compensated  using  the 
relative  motion  information  (provided,  for  example,  by  the  inertial  navigation)  due  to  the  unknown  image 
depth.  A  second  problem  is  "vergence,"  i.e.  the  fact  that  the  area  of  interest  must  be  in  the  field  of  view  (FoV) 
of  both  cameras,  and  must  be  focused  in  both  cameras.  This  typically  requires  different  orientation  of  the 
cameras  for  close  targets  (e.g.  more  "cross-eyed")  and  far  targets.  Differences  in  the  optical  response  of  the 
two  cameras  can  also  make  matching  more  difficult.  Monocular  vision  methods  give  a  scaled  estimate  of 
camera  motion  and  a  scaled  estimate  of  scene  structure.  This  scaled  estimate  is  the  major  drawback  of 
monocular  methods,  and  requires  additional  sensors  or  information  to  recover  the  correct  scale. 

The  fusion  of  vision  and  laser  data  is  applied  for  efficient  resolution  of  the  scale  ambiguity  and  performance 
enhancement  of  the  navigation  accuracy  and  reliability  in  the  face  of  image  noise.  Faser  scan  data  are 
exploited  to  initialize  three-dimensional  (3D)  tracking  of  stationary  features  and  to  enhance  the  feature 
tracking  performance.  Example  3D  features  include  planar  surfaces  (characterized  by  range  and  normal 
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vector)  and  point  features  (characterized  by  their  Cartesian  coordinates).  Changes  in  feature  parameters 
between  images  are  used  for  navigation.  Inertial  data  are  applied  to  perform  robust  feature  matching  between 
images  and  coasting  in  environments  where  insufficient  features  are  available.  INS  data  can  also  be  used  to 
adjust  laser  scan  range  measurements  for  platform  motion  in  order  to  compensate  for  the  time  discrepancy 
between  camera  images  and  laser  scans.  The  proposed  integration  concept  allows  for  the  initialization  of  3D 
feature  tracking  based  on  a  limited  number  of  laser  scans:  two  scans  are  required,  after  which  the  system  can 
operate  in  a  completely  passive  mode.  This  serves  as  an  important  aspect  for  many  surveillance, 
reconnaissance  and  navigation  missions. 

To  improve  the  robustness  of  the  navigation  solution,  the  vision/laser  integrated  mechanization  is  augmented 
with  inertial  sensor  measurements.  INS  outputs  are  exploited  for  robust  matching  of  the  features  that  are 
extracted  from  vision  and  laser  data.  The  use  of  INS  outputs  for  feature  matching  in  video  images  is  described 
in  [9].  Details  of  the  INS-based  feature  matching  in  laser  scan  images  are  discussed  in  [5].  Inertial  data  are 
also  used  to  coast  through  those  cases  where  a  limited  number  of  features  is  available.  Figure  13  shows  a 
high-level  diagram  of  the  Vision/Laser/INS  integrated  mechanization. 


Figure  13.  Vision/Laser/INS  Integrated  Solution 


Laser  scanner  measurements  are  used  to  initialize  the  depth  estimate  of  the  vision-based  features.  As  stated 
previously,  a  very  limited  number  of  scans  (two  scans)  is  sufficient  for  the  image  depth  initialization. 
Following  the  initialization  step,  the  system  can  operate  in  a  complete  passive  mode  using  vision-based 
features  only.  Navigation  accuracy  can  be  improved  considerably  if  periodic  scans  at  a  limited  rate  (such  as, 
for  example,  one  scan  per  30  s)  are  applied.  This  aspect  is  explained  in  more  details  in  the  feasibility 
demonstration  discussion  below.  For  those  cases  where  the  active  component  of  the  laser  measurement 
mechanism  does  not  represent  a  concern  (i.e.  for  the  majority  of  civil  use  cases),  laser  scanner  can  operate 
continuously  to  provide  additional  feature  measurements  in  order  to  improve  the  navigation  performance. 

The  vision/laser/INS  mechanization  shown  in  Figure  13  estimates  the  navigation  solution  (position,  velocity, 
and  attitude)  from  the  INS.  Vision  and  laser  feature  measurements  are  applied  to  estimate  inertial  error  states 
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in  order  to  mitigate  drift  in  inertial  navigation  outputs.  Laser-based  observables  of  the  Kalman  filter  are 
defined  by  equations  (12)  through  (15)  above. 


Equation  (22)  formulates  vision-based  observables  of  the  Kalman  filter  using  the  unit  sphere  representation  of 
image  features  that  supports  multi-aperture  camera  cases  [22]: 


„  Kalman 

fir 


(n<p2))T  •  AC^  •  B  ■  AR1NS  -  (ftp*)1 


8?) 


’  ACN  •  Dp  •  ARins 


(fi<P2)) 


AC’-n/; 


•  p(1) 


(22) 


P  =  1,-,P 


where: 

nps),  p  =  1,..,P,  s  =  1,2  is  the  unit  vectors  of  the  feature  p  (i.e.  the  unit  vector  pointed  from  the  center  of  the 
camera  to  the  feature  p)  that  is  extracted  from  image  s; 

n  ps)  is  the  unit  vector  that  is  perpendicular  to  the  feature  unit  vector;  pp  *  is  the  estimated  feature  range  for 
image  1; 

AR1NS  and  AC ^  are  INS  position  and  orientation  changes  between  images  1  and  2;  and, 
matrices  B  and  D  are  defined  as  follows: 


0 

-1  o’ 

0 

0 

— cos(cppP) 

B  = 

1 

0 

0 

DP= 

0 

0 

sin(q>p)) 

0 

0 

0 

cos(cpp  ^) 

sin((ppl)) 

0 

where: 

cpp  ’  is  the  feature  spherical  azimuth  angle  as  measured  from  image  1 . 


(23) 


Image  1  is  generally  the  image  where  the  feature  was  first  observed  and  image  2  is  the  current  image. 
Equation  (22)  is  linearized  to  support  the  linear  formulation  of  the  complementary  Kalman  filter. 

As  stated  previously  the  key  motivation  for  combining  vision  and  laser  data  is  the  use  of  laser  scan 
measurements  for  the  estimation  of  the  unknown  depth  of  video  images.  The  estimation  approach  is  illustrated 
in  Figure  14. 
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Figure  14.  Image  depth  initialization  method 


The  depth  estimation  exploits  two  images  from  a  video  camera  and  two  laser  scan  images  that  are  acquired  at 
two  different  locations.  It  is  assumed  that  the  camera  and  the  laser  are  mounted  on  the  same  platform  and  their 
measurement  frames  are  collocated.  Parameters  of  features  that  are  extracted  from  laser  scan  images  and 
video  images  at  two  locations  of  the  platform  are  used  to  estimate  the  image  depth.  The  estimation  does  not 
require  the  camera  and  the  laser  to  observe  the  same  features:  i.e.,  video  and  laser  features  can  be  completely 
unrelated  to  each  other.  The  initialization  method  discussed  in  this  paper  assumes  that  point  features  are 
extracted  from  video  images  and  lines  are  extracted  from  laser  scans.  This  method  can  be  generalized  to 
include  other  types  of  features. 

To  provide  a  conceptual  illustration  of  the  depth  initialization  method,  a  simplified  2D  case  is  considered 
below.  This  case  is  illustrated  in  Figure  15. 


Figure  15.  Illustration  of  the  depth  initialization  method:  simplified  2D  case 

Camera  and  laser  are  placed  on  the  same  moving  platform  and  their  measurement  frames  are  aligned.  The 
platform  moves  along  the  X  axis  of  the  XZ  navigation  coordinate  frame.  The  camera  optical  axis  is  aligned 
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with  the  Z  axis  of  the  frame.  For  simplicity,  it  is  assumed  that  the  camera  focal  length  F  equals  unity.  Feature 
1  is  observed  by  the  camera  and  feature  2  is  observed  by  the  laser  scanner.  As  stated  previously,  these  features 
may  be  completely  unrelated.  The  platform  displacement  AX  transforms  into  the  displacement  of  the  vision- 
based  feature  as: 


Ax(1)  =  AX/Z(1)  (24) 

where: 

Z(  1 1  is  the  unknown  depth. 

The  laser  scanner  observes  feature  2.  The  distance  to  this  feature  is  directly  measured  by  the  scanner:  2D  scan 
points  are  generally  represented  by  their  polar  coordinates  that  include  range  and  polar  angle.  Polar 
coordinates  of  feature  2  that  are  extracted  from  two  scan  images  are  applied  to  estimate  the  platform 
displacement: 

AX  =  -(p22)  sin(a(22))  -  p|2)  sin(a(|2))^  (25) 

where: 

and  (p22),a22)J  are  the  range  and  polar  angles  of  feature  2  in  the  first  and  second  scan  images, 
accordingly. 

Platform  displacement  that  is  estimated  based  on  scan  data  is  then  used  to  initialize  the  depth  of  the  image- 
based  feature: 

Z(1)  =  AX/Ax(1)  (26) 


A  similar  approach  is  applied  to  estimate  the  image  depth  for  a  general  3D  case.  Equation  (27)  relates  3D 
Cartesian  feature  coordinates  Mp  to  its  homogeneous  coordinates  mp  in  the  2D  image  frame: 


where: 


M«=Z«m« 


m(p2)  =  z(p2Wp2» 
p  =  i,-,p 


ZfkJ)  is  the  depth  of  the  kth  feature  in  the  jth  image. 


(27) 


Cartesian  feature  coordinates  in  two  images  are  related  through  the  orientation  change  matrix  and  the 
translational  vector: 

M(p2)  =  ACb  (M™ -  AR)  (28) 

Substituting  equation  (27)  into  (28)  then  yields: 

M(p2)  =  AC?(zJ)-m;)-AR)  (29) 

Assuming  that  the  optical  axis  is  aligned  with  the  Z-  axis  of  the  camera  frame,  the  depth  of  the  feature  is 
related  to  its  Cartesian  coordinates  as  follows: 

Z®  =  Hz  •  M®  (30) 


RTO-EN-SET-1 16(2010) 


9-21 


Navigation  in  Difficult 

Environments:  Multi-Sensor  Fusion  Techniques 


ORGANIZATION 


where: 

nz  =  [0  0  if. 

From  equations  (27)  through  (30),  the  following  relationship  can  be  derived  [23]: 


where: 


AR 


=  0 


2x1 


Hp  =  A  (l3x3  -  AC*  •  m(k2)  •  (nTz  AC*  ))  [l3x3 
p  =  l,...,P 


I3x3  is  the  3-by-3  unit  matrix;  and, 
02xi  is  a  2x1  zero  column. 


(31) 


Equation  (31)  formulates  vision-based  linear  equations  for  the  estimation  of  the  image  depth.  Note  that 
unknowns  include  the  depth  components  (Z^,  p=l,...,P)  and  the  displacement  vector  AR.  The  orientation 

change  matrix  is  computed  from  the  inertial  data  and  is  not  estimated  based  on  the  laser/vision  fusion.  This 
allows  for  the  linear  formulation  of  the  depth  estimation  problem.  Note  that  in  this  case  a  correlation  between 
inertial  angular  errors  and  initial  depth  estimates  is  created.  This  correlation  must  be  taken  into  account  in  the 
design  of  the  Kalman  filter  that  estimates  inertial  drift  states. 

The  solution  of  the  equation  system  (31)  is  not  unique.  The  displacement  vector  and  depth  can  only  be 
determined  within  an  ambiguity  of  a  scale-factor  y:  i.e.,  if  AR  and  Z(1)  satisfy  the  system  (31),  then  yAR  and 

yZ(pn  satisfy  this  system  as  well.  To  remove  the  scale-factor  ambiguity,  the  system  (31)  is  augmented  by  laser 

measurement  observables.  Line  features  are  extracted  from  laser  images  and  applied  for  the  image  depth 
initialization.  It  is  assumed  herein  that  lines  are  created  in  2D  scan  images  as  a  result  of  the  intersection  of  the 
horizontal  laser  scanning  plane  with  vertical  planes  (such  as  building  walls  in  urban  environments).  In  this 
case,  the  relationship  between  changes  in  line  parameters  and  displacement  vector  components  is  formulated 
by  equation  (11)  above,  which,  in  the  matrix  form,  is  expressed  as  follows: 

nTAn-n(1Ln(2) 
nk  AR  —  Pk  Pk 

nk  =  [cos(a(kn)  sin(akl))]  (32) 

k  =  l,..,K 

Equation  (32)  provides  the  laser  part  of  the  mage  depth  estimation  relations.  Combining  (32)  with  the  vision- 
based  part  defined  by  equation  (3 1)  yields: 


H. 


AR 

7<l) 

% 


02xl,p  =  l,...,P 


nkAR  =  Pk*  _Pk2),k  =  1,...,K 


(33) 
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Equation  (33)  defines  a  linear  system  of  equations  for  the  estimation  of  image  depth.  Note  that  the  minimum 
feature  configuration  that  is  required  to  initialize  the  image  depth  based  on  equation  (33)  includes  three  vision 
features  and  one  laser  feature.  In  this  case,  seven  equations  are  available  (two  per  vision  feature  and  one  for 
the  laser  feature)  to  estimate  six  unknowns  (three  depth  values  and  three  components  of  the  displacement 
vector). 

The  image  depth  initialization  method  was  verified  with  experimental  data.  Indoor  experimental  data  were 
collected  in  hallways  of  the  Air  Force  Research  Laboratory  at  Eglin  Air  Force  Base.  Figure  16  shows  a 
photograph  of  the  data  collection  setup. 


Figure  16.  Data  collection  setup  used  for  demonstrating 
the  feasibility  of  the  laser/vision  depth  estimation 


The  data  collection  setup  includes  a  laser  scanner  and  three  video  cameras.  This  setup  was  assembled  for  the 
development  and  verification  of  the  laser  scanner/multi-aperture  video  data  fusion.  Only  one  video  camera 
was  used  for  the  experiment  discussed  herein.  A  straight  motion  trajectory  was  implemented.  Two  laser  scans 
were  used  to  initialize  the  depth  of  video  images.  Following  the  depth  initialization,  the  displacement  vector 
was  estimated  based  on  vision  features  only  without  the  use  of  laser  scan  data.  Vision-based  displacement 
estimates  were  compared  to  the  reference  motion  trajectory.  To  construct  the  reference  trajectory,  cm-accurate 
displacement  information  was  estimated  from  laser  scan  images  as  described  in  reference  [5],  Figure  17 
exemplifies  experimental  results. 

Experimental  data  presented  show  that  decimeter-level  positioning  accuracy  that  is  achieved  using  video 
images  and  two  laser  scans  to  initialize  the  image  depth.  These  results  clearly  demonstrate  the  feasibility  of 
the  semi-passive  navigation  approach  that  uses  very  limited  laser  scans  (two  scans  for  the  example  case 
considered  herein)  for  the  initialization  of  3D  image-based  navigation. 
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Figure  17.  Example  of  the  vision/laser  trajectory  estimation  performance:  only  two  laser  scans 
are  applied  to  initialize  the  image  depth  (at  the  system  start-up  and  at  about  5  seconds  after 
the  beginning  of  the  experiment);  following  the  depth  initialization,  3D  position 
components  are  estimated  based  on  video  images  only 

Feasibility  of  the  vision/laser/INS  mechanization  described  above  was  assessed  in  an  indoor  simulated 
environment  that  was  implemented  in  Matlab.  For  the  simulation,  the  sensor  specifications  were  implemented 
as  follows: 

•  Video:  a  multi-aperture  camera  head  that  includes  three  video  cameras  (see  Figure  16  for  the 
illustration  of  the  multi-aperture  implementation);  the  angular  separation  between  the  camera  optical 
axis  is  120  deg;  for  each  camera  the  resolution  is  640x480,  the  azimuth  FoV  is  40  deg,  the  elevation 
FoV  is  40  deg,  and,  the  update  rate  is  10  Hz; 

•  Laser  scanner:  measurement  range  is  80  m;  angular  range:  is  from  0  to  360  deg;  range  noise  is  1  cm 
(std);  and,  angular  resolution  is  0.5  deg; 

•  Inertial  measurement  unit:  accelerometer  bias  is  1  mg;  and,  gyro  drift  is  50  deg/hr. 

Figure  18  illustrates  a  2D  horizontal  projection  of  the  simulated  indoor  environment  that  includes  multiple 
indoor  hallways.  Hallway  walls  were  simulated  as  vertical  with  the  wall  height  equal  to  2.5  m.  A  horizontal 
motion  trajectory  with  the  absolute  velocity  value  of  1  m/s  was  implemented  as  shown  in  Figure  18. 
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Simulated  indoor  test 


Figure  18.  Simulated  indoor  environments 

Figure  19  shows  performance  of  the  vision/inertial  integration,  i.e.,  the  use  of  laser  measurements. 
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Figure  19.  Performance  of  the  vision/inertial  integration  for  the  indoor  simulated  environment 

In  this  case,  the  feature  depth  is  initialized  by  observing  the  feature  from  two  different  location  of  the  platform 
and  using  inertial  displacement  and  orientation  change  measurements  for  the  unambiguous  estimation  of  the 
image  depth  as  described  in  [22].  Simulation  results  shown  in  Figure  19  indicate  that  position  errors  of  the 
vision/INS  integration  stay  in  the  range  from  -10  m  to  20  m.  This  level  of  navigation  performance  is  improved 
significantly  if  laser  scanner  measurements  are  incorporated  into  the  solution.  Figure  20  illustrates 
performance  of  the  vision/laser/INS  mechanization  for  the  case  where  a  1-s  update  rate  of  laser  images  is 
used. 
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Position  errors:  Vision/laser/inertial  integration 


Figure  20.  Performance  of  the  vision/inertial  integration  for 
the  indoor  simulated  environment:  1-s  laser  updates 

The  use  of  a  1-s  laser  updates  enables  cm-accurate  estimation  of  the  3D  position  vector.  Figure  21  shows 
simulation  results  for  the  case  where  periodic  laser  scans  at  a  very  limited  rate  are  applied.  In  this  case,  laser 
scans  are  made  only  once  per  30  s. 


Position  errors:  Vision/laser/inertial  integration 
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Figure  21.  Performance  of  the  vision/inertial  integration  for 
the  indoor  simulated  environment:  30-s  laser  updates 


It  is  important  to  note  that  the  system  that  employs  laser  scans  at  this  low  rate  can  be  still  considered  as 
practically  passive  since,  from  the  practical  point  of  view,  a  laser  scanning  at  this  rate  cannot  be  detected. 
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Positioning  accuracy  is  maintained  at  a  sub-m-level,  which  provides  an  order  of  magnitude  performance 
improvement  as  compared  to  the  vision/laser  implementation.  Therefore,  for  the  vision/laser/INS  integration  it 
is  extremely  beneficial  to  use  the  system  implementation  that  operates  in  a  semi-passive  mode  employing 
periodic  scans  at  a  limited  scan  rate. 

4.0  SUMMARY 

Navigating  in  difficult  environments  requires  the  use  of  multi-sensor  fusion  techniques.  This  paper  proposes  a 
generic  multi-sensor  fusion  approach  and  applies  this  approach  for  developing  GPS/laser  scanner/inertial  and 
vision/laser/inertial  integrated  mechanizations.  Simulated  data  and  data  collected  in  various  urban  indoor  and 
outdoor  environments  show  that  multi-sensor  fusion  techniques  developed  demonstrate  a  significant  potential 
for  enabling  reliable  and  accurate  navigation  capabilities  for  a  variety  of  challenging  navigation  scenarios. 

DISCLAIMER 

The  views  expressed  in  this  article  are  those  of  the  authors  and  do  not  reflect  the  official  policy  or  position  of 
the  United  States  Air  Force,  Department  of  Defense,  or  the  U.S  Government. 
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